#ifndef _lin_func_detail_
#define _lin_func_detail_

#include <string>
#include <cmath>
#include <cstdlib>
#include <eigen3/Eigen/Core>

Eigen::VectorXd func(Eigen::VectorXd _u)
{
    double mu = 1.0 / 81.45;
    Eigen::VectorXd u_f(6);
    u_f[0] = _u[3];
    u_f[1] = _u[4];
    u_f[2] = _u[5];
    double t2 = _u[0] + mu - 1.0;
    double t3 = _u[1] * _u[1] + _u[2] * _u[2] + t2 * t2;
    double t4 = sqrt(t3 * t3 * t3);
    double t5 = _u[1] * _u[1] + _u[2] * _u[2] + (t2 + 1.0) * (t2 + 1.0);
    double t6 = sqrt(t5 * t5 * t5);
    u_f[3] = 2.0 * _u[4] + _u[0] - mu * t2 / t4 - (1.0 - mu) * (t2 + 1.0) / t6;
    u_f[4] = -2.0 * _u[3] + _u[1] - mu * _u[1] / t4 - (1.0 - mu) * _u[1] / t6;
    u_f[5] = - mu * _u[2] / t4 - (1.0 - mu) * _u[2] / t6;
    return u_f;
};

Eigen::MatrixXd Jfunc(Eigen::VectorXd _u)
{
    double mu = 1.0 / 81.45;
    Eigen::MatrixXd u_Jf;
    u_Jf.resize(6,6);
    
    double sum1 = (_u(0) + 20.0/1629) * (_u(0) + 20.0/1629) + _u(1) * _u(1) + _u(2) * _u(2);
    double sum2 = (_u(0) - 1609.0/1629) * (_u(0) - 1609.0/1629) + _u(1) * _u(1) + _u(2) * _u(2);
    double sum1_32 = sqrt(sum1) * sum1;
    double sum1_52 = sum1_32 * sum1;
    double sum2_32 = sqrt(sum2) * sum2;
    double sum2_52 = sum2_32 * sum2;
    
    u_Jf.col(0) << 0, 0, 0,
    1 + 1.5*(0.9877225292*_u(0) + 0.01212673455)*(2.0*_u(0) + 0.02455494168)/sum1_52 - 0.01227747084/sum2_32 - 0.9877225292/sum1_32 + 1.5 * (2.0*_u(0) - 1.975445058)*(0.01227747084*_u(0) - 0.01212673455)/sum2_52,
    1.481583794 * _u(1) * (2.0 * _u(0) + 0.02455494168)/sum1_52 + 0.01841620626 * _u(1) * (2.0 * _u(0) - 1.975445058)/sum2_52,
    1.481583794 * _u(2) * (2.0 * _u(1) + 0.02455494168)/sum1_52 + 0.01841620626 * _u(2) * (2.0 * _u(0) - 1.975445058)/sum2_52;
    u_Jf.col(1) << 0, 0, 0,
    3 * _u(1) * (0.9877225292 * _u(0) + 0.01212673455)/sum1_52 + 3 * _u(1) * (0.01227747084 * _u(0) - 0.01212673455)/sum2_52,
    1 + 2.963167587 * _u(1) * _u(1) /sum1_52 - 0.01227747084/sum2_32 - 0.9877225292/sum1_32 + (0.03683241252 * _u(1) * _u(1))/sum2_52,
    2.963167587 * _u(1) * _u(2) /sum1_52 + (0.03683241252 * _u(1) * _u(2))/sum2_52;
    u_Jf.col(2) << 0, 0, 0,
    3 * _u(2) * (0.9877225292 * _u(0) + 0.01212673455)/sum1_52 + 3 * _u(2) * (0.01227747084 * _u(0) - 0.01212673455)/sum2_52,
    2.963167587 * _u(1) * _u(2) /sum1_52 + (0.03683241252 * _u(1) * _u(2))/sum2_52,
    2.963167587 * _u(2) * _u(2) /sum1_52 - 0.01227747084/sum2_32 - 0.9877225292/sum1_32 + (0.03683241252 * _u(2) * _u(2))/sum2_52;
    u_Jf.col(3) << 1, 0, 0, 0, -2, 0;
    u_Jf.col(4) << 0, 1, 0, 2, 0, 0;
    u_Jf.col(5) << 0, 0, 1, 0, 0, 0;

    return u_Jf;
};

#else
//DO NOTHING
#endif
